// Magic Software, Inc.
// http://www.magic-software.com
// Copyright (c) 2000, All Rights Reserved
//
// Source code from Magic Software is supplied under the terms of a license
// agreement and may not be copied or disclosed except in accordance with the
// terms of that agreement.  The various license agreements may be found at
// the Magic Software web site.  This file is subject to the license
//
// FREE SOURCE CODE
// http://www.magic-software.com/License/free.pdf

#include "MgcRK4.h"

//---------------------------------------------------------------------------
MgcRK4::MgcRK4 (int iDim, MgcReal fStep, Function* aoF)
    :
    MgcODE(iDim,fStep,aoF)
{
    m_fHalfStep = 0.5*fStep;
    m_fSixthStep = fStep/6.0;
    m_afTemp1 = new MgcReal[iDim];
    m_afTemp2 = new MgcReal[iDim];
    m_afTemp3 = new MgcReal[iDim];
    m_afTemp4 = new MgcReal[iDim];
    m_afXTemp = new MgcReal[iDim];
}
//---------------------------------------------------------------------------
MgcRK4::MgcRK4 (int iDim, MgcReal fStep, AutoFunction* aoA)
    :
    MgcODE(iDim,fStep,aoA)
{
    m_fHalfStep = 0.5*fStep;
    m_fSixthStep = fStep/6.0;
    m_afTemp1 = new MgcReal[iDim];
    m_afTemp2 = new MgcReal[iDim];
    m_afTemp3 = new MgcReal[iDim];
    m_afTemp4 = new MgcReal[iDim];
    m_afXTemp = new MgcReal[iDim];
}
//---------------------------------------------------------------------------
MgcRK4::~MgcRK4 ()
{
    delete[] m_afTemp1;
    delete[] m_afTemp2;
    delete[] m_afTemp3;
    delete[] m_afTemp4;
    delete[] m_afXTemp;
}
//---------------------------------------------------------------------------
void MgcRK4::Update (MgcReal fTIn, MgcReal* afXIn, MgcReal& rfTOut,
    MgcReal* afXOut)
{
    // first step
    int i;
    for (i = 0; i < m_iDim; i++)
        m_afTemp1[i] = m_aoF[i](fTIn,afXIn);
    for (i = 0; i < m_iDim; i++)
        m_afXTemp[i] = afXIn[i] + m_fHalfStep*m_afTemp1[i];

    // second step
    MgcReal fHalfT = fTIn + m_fHalfStep;
    for (i = 0; i < m_iDim; i++)
        m_afTemp2[i] = m_aoF[i](fHalfT,m_afXTemp);
    for (i = 0; i < m_iDim; i++)
        m_afXTemp[i] = afXIn[i] + m_fHalfStep*m_afTemp2[i];

    // third step
    for (i = 0; i < m_iDim; i++)
        m_afTemp3[i] = m_aoF[i](fHalfT,m_afXTemp);
    for (i = 0; i < m_iDim; i++)
        m_afXTemp[i] = afXIn[i] + m_fStep*m_afTemp3[i];

    // fourth step
    rfTOut = fTIn + m_fStep;
    for (i = 0; i < m_iDim; i++)
        m_afTemp4[i] = m_aoF[i](rfTOut,m_afXTemp);
    for (i = 0; i < m_iDim; i++)
    {
        afXOut[i] = afXIn[i] + m_fSixthStep*(m_afTemp1[i] +
            2.0*(m_afTemp2[i] + m_afTemp3[i]) + m_afTemp4[i]);
    }
}
//---------------------------------------------------------------------------
void MgcRK4::Update (MgcReal* afXIn, MgcReal* afXOut)
{
    // first step
    int i;
    for (i = 0; i < m_iDim; i++)
        m_afTemp1[i] = m_aoA[i](afXIn);
    for (i = 0; i < m_iDim; i++)
        m_afXTemp[i] = afXIn[i] + m_fHalfStep*m_afTemp1[i];

    // second step
    for (i = 0; i < m_iDim; i++)
        m_afTemp2[i] = m_aoA[i](m_afXTemp);
    for (i = 0; i < m_iDim; i++)
        m_afXTemp[i] = afXIn[i] + m_fHalfStep*m_afTemp2[i];

    // third step
    for (i = 0; i < m_iDim; i++)
        m_afTemp3[i] = m_aoA[i](m_afXTemp);
    for (i = 0; i < m_iDim; i++)
        m_afXTemp[i] = afXIn[i] + m_fStep*m_afTemp3[i];

    // fourth step
    for (i = 0; i < m_iDim; i++)
        m_afTemp4[i] = m_aoA[i](m_afXTemp);
    for (i = 0; i < m_iDim; i++)
    {
        afXOut[i] = afXIn[i] + m_fSixthStep*(m_afTemp1[i] +
            2.0*(m_afTemp2[i] + m_afTemp3[i]) + m_afTemp4[i]);
    }
}
//---------------------------------------------------------------------------
void MgcRK4::SetStepSize (MgcReal fStep)
{
    m_fStep = fStep;
    m_fHalfStep = 0.5*fStep;
    m_fSixthStep = fStep/6.0;
}
//---------------------------------------------------------------------------

#ifdef RK4_TEST

#include "MgcRTLib.h"

MgcReal F0 (MgcReal fT, MgcReal* afX) { return afX[0]*afX[0]; }
MgcReal F1 (MgcReal fT, MgcReal* afX) { return -2.0*afX[0]*afX[1]; }

int main ()
{
    const int iDim = 2;
    const MgcReal fStep = 0.001;
    MgcODE::Function aoF[2] = { F0, F1 };

    MgcRK4 kODE(iDim,fStep,aoF);
    MgcReal fTIn = 0.0, fTOut;
    MgcReal afXIn[iDim] = { 1.0, 1.0 }, afXOut[iDim];

    for (int i = 0; i < 10; i++)
    {
        cout << "t = " << fTIn << ' ';
        cout << "x = " << afXIn[0] << ' ';
        cout << "y = " << afXIn[1] << endl;
        kODE.Update(fTIn,afXIn,fTOut,afXOut);
        for (int j = 0; j < iDim; j++)
            afXIn[j] = afXOut[j];
        fTIn = fTOut;
    }

    // t = 0 x = 1 y = 1
    // t = 0.001 x = 1.001 y = 0.998001
    // t = 0.002 x = 1.002 y = 0.996004
    // t = 0.003 x = 1.00301 y = 0.994009
    // t = 0.004 x = 1.00402 y = 0.992016
    // t = 0.005 x = 1.00503 y = 0.990025
    // t = 0.006 x = 1.00604 y = 0.988036
    // t = 0.007 x = 1.00705 y = 0.986049
    // t = 0.008 x = 1.00806 y = 0.984064
    // t = 0.009 x = 1.00908 y = 0.982081

    return 0;
}

#endif
